#ifndef CREATURE_H
#define CREATURE_H

#include <fstream>
#include <vector>
#include <opal.h>
#include "BoxEntity.h"
#include "SphereEntity.h"
#include "CapsuleEntity.h"
#include <string>
#include "SimSolid.h"

class Creature{



	int defaultJoints;

	std::vector<SimSolid *> simSolids;

	opal::Simulator* gSimulator;
	std::vector<opalSamples::Entity*> *gEntities;
	//std::string filename;

	std::vector<opal::Point3r> startStates;
	std::vector<opal::Quaternion> quaternions;

public:

	std::vector<opal::Solid *> solids;
	std::vector<opal::Joint *> joints;
	std::vector<opal::Motor *> motors;
	std::vector<opal::Motor *> servos;
	std::vector<opal::Motor *> motors1;

	bool isPartofState;

	Creature(opal::Simulator* sim, std::vector<opalSamples::Entity*> *entities){//, std::string pfilename){
		gSimulator = sim;
		gEntities = entities;
	//	filename = pfilename;
		//solids = new std::vector<opal::Solid *> ();
	//	joints = new std::vector<opal::Joint *> ();
	}

	void addJoint(opal::Joint *j);
	void addSolid(opal::Solid *s);

	int getNumJoints(){
		return joints.size();
	}

	int getNumSolids(){
		return solids.size();
	}

	void applyForce(int solidNum, opal::Vec3r v);
	void applyServoForce(int jointNum, double jointAngle);
	void applyGearedForce(int jointNum, double throttle);

	void create(SimSolid *s);

void readAgent();
//void createSphere(opal::Solid** solid);
void createSphere(opal::Solid** solid, opal::Matrix44r transform, double radius);
void createBox(opal::Solid** solid, opal::Matrix44r transform, opal::Vec3r v);
void createCapsule(opal::Solid** solid, opal::Matrix44r transform, double radius, double length);
void createJoint(opal::Joint **joint, opal::Solid *solid0, opal::Solid *solid1);
void createAgent(void);
opal::Point3r getAgentPosition(int numSolid);
opal::real randomRealUniform(opal::real min, opal::real max);

void setAgentPosition(int numSolid, opal::Point3r);
void removeForces(void);

void setOneStartState (int numSolid, opal::Point3r);
void resetPositions(opal::Point3r minWorld, opal::Point3r maxWorld);
opal::Point3r getStartState(int numSolid);
void setStartStates();

void joinSolids(int solid0, int solid1);
void breakJoints(int jointNum);

};

#endif
